#!/usr/bin/python
# -*- coding: UTF-8 -*-

import rospy
import time,cv2
import numpy as np
import os
import re

from std_msgs.msg import String
from nav_msgs.msg import OccupancyGrid,MapMetaData
from cln_msgs.msg import CleanTask,MapStatus,MapIndex
from cln_msgs.srv import MapSearch,MapSearchRequest,MapSearchResponse

# from tool import User_Data_Server
class BulkdataParser:
    def __init__(self):
        self.__node_name = "BulkdataParser"

        ## 用户参数配置服务
        # self.__user_data_server = User_Data_Server()
        ##地图名缓存
        self.__map_name = "" 
        self.__map_isupdate = False
        self.__map_index_info = {}

        # 局部地图更新时候的名字
        self.__map_name_update = "" 
        # 用户地图旋转角度
        self.__user_rotate_angle = 0 

        ## 清扫轨迹缓存
        self.__clean_task_map_isupdate = False
        ## 建图状态
        self.__in_creating_map = False
        
        ##地图配置信息缓存
        # self.__map_info_size = 0
        self._map_info_originx = 0
        self._map_info_originy = 0
        self._map_info_width = 0
        self._map_info_height = 0
        self._map_info_resolution = 0
  
        ##地图信息缓存
        self.map_size = 0
        self.map_originx = 0
        self.map_originy = 0
        self.map_width = 0
        self.map_height = 0
        self.resolution=0

        ##机器清扫的任务路线地图信息缓存
        self.task_path_map_size = 0
        self.task_path_map_originx = 0
        self.task_path_map_originy = 0
        self.task_path_map_width = 0
        self.task_path_map_height = 0

        ## 当前任务信息
        self.task = CleanTask()  

    def InitParser(self, node_name):
        '''
            初始化
        '''
        ## node name配置
        self.__node_name = node_name

        ## ros param
        self.__map_create_resolution = rospy.get_param('map_create_resolution',0.1) # 建图分辨率
        
        self.__map_database_dir = rospy.get_param('maps_dir','/home/aoting/user_config/map_database/')
        
        if not os.path.exists(self.__map_database_dir):
            os.makedirs(self.__map_database_dir)
        
        self.__user_config_dir = rospy.get_param("user_config","/home/aoting/user_config/")
        self.__maps_cache_dir = rospy.get_param('maps_cache_dir','/home/aoting/user_config/cache_database/maps_cache/')
        
        if not os.path.exists(self.__maps_cache_dir):
            os.makedirs(self.__maps_cache_dir)
        
        self.__mapcache_path = os.path.join(self.__maps_cache_dir, "map_cache.png")
        ## 任务路线地图缓存路径
        self.__task_path_mapcache_dir = rospy.get_param('task_execution_path_cache_dir','/home/aoting/user_config/cache_database/task_execution_path_cache/') 

        if not os.path.exists(self.__task_path_mapcache_dir):
            os.makedirs(self.__task_path_mapcache_dir)
            
        self.__task_path_mapcache_path = os.path.join(self.__task_path_mapcache_dir, "task_path_map_cache.png")
        
        ##订阅topic 
        rospy.Subscriber("/map_name",String,self.MapName_Callback,queue_size=1)
        ### 订阅机器人建图状态信息
        rospy.Subscriber("/map_status",MapStatus,self.MapStaus_Callback,queue_size=1)
        ### 订阅机器人建图时,地图信息（地图路径，地图元数据）
        rospy.Subscriber("/map_index",MapIndex,self.MapIndex_Callback,queue_size=1)
        ## 机器清扫的任务路线
        rospy.Subscriber("/clean_mask",OccupancyGrid,self.TaskPathMap_Callback,queue_size=1)
        ## 订阅实时任务信息
        rospy.Subscriber("/task_echo",CleanTask,self.ExecutingTask_Callback,queue_size=1)
        ## 订阅当前地图数据信息
        rospy.Subscriber("/map_metadata",MapMetaData,self.MapInfo_Callback,queue_size=1)    # 20230824-dw.liu
        ## 局部地图更新的时候，发布正在更新的地图名
        rospy.Subscriber("/map_name_update",String,callback=self.MapNameUpdate_Callback)
        
        self.__get_mapinfo_client=rospy.ServiceProxy('mapsearch_manager',MapSearch)
        


    def GetNodeName(self):
        '''
            获取节点名称
        '''
        return self.__node_name

    #-----------------------------------#
    #******#  ROS topic Callback #******# 
    #-----------------------------------#

    def MapStaus_Callback(self, msg_data):
        '''
        建图状态回调函数
        '''
        self.__in_creating_map = msg_data.in_creating
        # if not self.__in_creating_map and os.path.exists(self.__mapcache_path): # 清空缓存文件
        #     os.remove(self.__mapcache_path)
        if not self.__in_creating_map: # 清空缓存文件
            self.CleanMapCache_all()

    
    def MapCachePath_Callback(self,msg):# 20230607-dw.liu
        '''
        机器人建图时map_cache_path
        '''
        self.__mapcache_path = msg.data
        
        
        

    def ExecutingTask_Callback(self, task):
        #print("task.type = {}, task.task_name= {},task.state = {}".format(task.type,task.task_name,task.state))
        self.task = task
        #任务完成

    def MapInfo_Callback(self, msg_data):
            '''
            订阅当前地图数据信息
            目的机器人没有处于建图的时候，为pad接口/map/CloudImgUpdate提供地图数据
            20230824-dw.liu
            '''
            # print(msg_data)
            self._map_info_originx = msg_data.origin.position.x
            self._map_info_originy = msg_data.origin.position.y
            self._map_info_width = msg_data.width
            self._map_info_height = msg_data.height
            self._map_info_resolution = msg_data.resolution

    def MapIndex_Callback(self, msg):
        '''
        Map地图回调函数
        '''
        try:
            self.__map_index_info["data"] = {}
            self.__map_index_info["data"]["position_x"] = msg.map_meta.origin.position.x
            self.__map_index_info["data"]["position_y"] = msg.map_meta.origin.position.y
            self.__map_index_info["data"]["resolution"] = msg.map_meta.resolution
            self.__map_index_info["data"]["width"] = msg.map_meta.width
            self.__map_index_info["data"]["height"] = msg.map_meta.height
            self.__map_index_info["data"]["user_rotate_angle"] = self.GetUserRotateAngle()
            self.__map_index_info["data"]["pose_x"] = msg.robot_pose.x
            self.__map_index_info["data"]["pose_y"] = msg.robot_pose.y
            self.__map_index_info["data"]["pose_yaw"] = msg.robot_pose.theta
            self.__map_index_info["data"]["extend_top_bottom"] = msg.extend_top_bottom
            self.__map_index_info["data"]["extend_left_right"] = msg.extend_left_right
            self.__map_index_info["map_png_str"] = msg.map_png_str

            # # 对应/map_index
            # self.map_originx = msg_data.map_meta.origin.position.x
            # self.map_originy = msg_data.map_meta.origin.position.y
            # self.map_width = msg_data.map_meta.width
            # self.map_height = msg_data.map_meta.height
            # self.resolution = msg_data.map_meta.resolution
            # self.__map_base64 = msg_data.map_base64
            # self.__mapcache_path = msg_data.map_path
            
        except Exception as err:
            rospy.logerr("[BulkdataParser][Map_Callback]Map generation error: {}".format(str(err)))
      
    
    
    
    def MapName_Callback(self,msg):
        '''
        当前Map地图名称回调函数
        '''
        self.__map_name = msg.data
        rospy.loginfo("[BulkdataParser]Map name: {}".format(self.__map_name))

    
    
    
    
    def TaskPathMap_Callback(self,msg_data):
        '''
        清扫任务路径（图）
        '''
        try:
            # 这个回调函数的话题一直在发布
            #if msg_data.info.width != 0:

            map_size = msg_data.info.width * msg_data.info.height
            if map_size == 0 or self.task.state == CleanTask.NEWBORN:
                if os.path.exists(self.__task_path_mapcache_path):
                    os.remove(self.__task_path_mapcache_path)  # 清空任务路径缓存
                return
        
            self.task_path_map_size = map_size
            self.task_path_map_originx = msg_data.info.origin.position.x
            self.task_path_map_originy = msg_data.info.origin.position.y
            self.task_path_map_width = msg_data.info.width
            self.task_path_map_height = msg_data.info.height

            map_np = np.array(msg_data.data).reshape(msg_data.info.height,msg_data.info.width)
            max_np = np.array([255 for i in range(map_size)]).reshape(msg_data.info.height,msg_data.info.width)

            map_np = max_np - map_np*2.55

            map_np[map_np>255]=128

            map_np = cv2.flip(map_np,0)
        
            self.__clean_task_map_isupdate = True
            cv2.imwrite(self.__task_path_mapcache_path,map_np)  
            self.__clean_task_map_isupdate = False

        except Exception as err:
            rospy.logerr("[BulkdataParser][TaskPathMap_Callback]Map generation error: {}".format(str(err)))
    
    def MapNameUpdate_Callback(self,msg):
        '''
        局部地图更新的时候，发布正在更新的地图名
        '''
        self.__map_name_update = msg.data

        mapinfo_req = MapSearchRequest()
        mapinfo_req.map_name = self.__map_name_update
        mapinfo_res = self.__get_mapinfo_client.call(mapinfo_req)

        if  mapinfo_res.success: 
            pass
        mapinfo = mapinfo_res.map_info_msg
        self.__user_rotate_angle = mapinfo.rotate_angle

        
        # # 获取旋转角度
        # map_rotate_angle_info = self.__user_data_server.ReadMapRotateAngleParams()
        # for  map_rotate_angle_one in map_rotate_angle_info:
        #         if map_rotate_angle_one["map_name"] == self.__map_name_update:
        #             self.__user_rotate_angle =   map_rotate_angle_one["user_rotate_angle"] 
    #-----------------------------------#
    #******#  Interface function #******# 
    #-----------------------------------#
    def GetCurMapName(self):
        '''
        获取当前Map地图名称
        '''
        return self.__map_name

    def IsMapUpdating(self):
        '''
        是否正在地图更新中
        '''
        return self.__map_isupdate
    
    def IsCleanTaskMapUpdating(self):
        '''
        是否正在清扫任务路径更新中
        '''
        return self.__clean_task_map_isupdate
    
    def IsMapCreating(self):
        '''
        是否处于建图中
        '''
        return self.__in_creating_map

    def GetMapcacheDir(self):
        '''
        获取地图缓存dir
        '''
        return self.__mapcache_path

    def GetMapDatabasePath(self):
        '''
        获取地图数据库path
        '''
        return self.__map_database_dir

    def GetMapIndex(self):
        '''

        '''
        return self.__map_index_info

    def GetTaskPathMapcacheDir(self):
        '''
        获的任务路径地图缓存dir
        '''
        return self.__task_path_mapcache_path
    
    def GetUserRotateAngle(self):
        '''
        获取用户旋转角度
        '''
        return self.__user_rotate_angle
    
    def GetMapCreateResolution(self):
        '''
        获取建图分辨率
        '''
        return self.__map_create_resolution
    
    
    def CleanMapCache(self,map_cache_path_now): #20230607-dw.liu
        '''
        功能函数，清空/user_config/cache_database/文件下多余的png图片
        '''
        map_cache_path = self.__maps_cache_dir
        # os.path.join(self.__user_config_dir, "cache_database")
        
        ls = os.listdir(map_cache_path)
        for i in ls:
            f_path = os.path.join(map_cache_path, i)
            # print("====")
            # print(f_path)
            if f_path.find("png") != -1 and f_path != map_cache_path_now:
                os.remove(f_path)
                # print("=========")
                # print(f_path)
    
    def CleanMapCache2(self,map_cache_path_now): #20230613-dw.liu
        '''
        功能函数，清空/user_config/cache_database/文件下,时间戳靠前的png图片（png图片以时间戳命名）
        '''
        map_cache_path = self.__maps_cache_dir
        # os.path.join(self.__user_config_dir, "cache_database")
        re_search_ret = re.search('\d+', map_cache_path_now)
        if re_search_ret:
            map_cache_path_now_num = re_search_ret.group()
            # map_cache_path_now_num = re.search('\d+', map_cache_path_now).group()
            # print(type(map_cache_path_now_num))
            # print(map_cache_path_now_num)
            map_cache_path_now_num  = int(map_cache_path_now_num)
            ls = os.listdir(map_cache_path)
            for i in ls:
                f_path = os.path.join(map_cache_path, i)
                # print("====")
                # print(f_path)
                re_search_ret_2 = re.search('\d+', f_path)
                if re_search_ret_2:
                    if f_path.find("png") != -1 and int(re_search_ret_2.group()) < map_cache_path_now_num:
                        os.remove(f_path)
                        # print("=========")
                        # print(f_path)
    
    def CleanMapCache3(self,map_cache_path_now): #20230613-dw.liu
        '''
        功能函数，清空/user_config/cache_database/文件下,时间戳靠前的png图片（png图片以时间戳命名）
        '''
        map_cache_path = self.__maps_cache_dir
        # os.path.join(self.__user_config_dir, "cache_database")
        map_cache_path_now_num = re.search('\d+', map_cache_path_now).group()
        # print(type(map_cache_path_now_num))
        # print(map_cache_path_now_num)
        map_cache_path_now_num  = int(map_cache_path_now_num) - 10000 # 10s(秒)
        ls = os.listdir(map_cache_path)
        for i in ls:
            f_path = os.path.join(map_cache_path, i)
            # print("====")
            # print(f_path)
            if f_path.find("png") != -1 and int(re.search('\d+', f_path).group()) < map_cache_path_now_num:
                os.remove(f_path)
                # print("=========")
                # print(f_path)
    
    def CleanMapCache_all(self): #20230613-dw.liu
        '''
        功能函数，结束建图后，清空/user_config/cache_database/文件下,所有的png图片
        '''
        map_cache_path = self.__maps_cache_dir
        # os.path.join(self.__user_config_dir, "cache_database")
        
        ls = os.listdir(map_cache_path)
        for i in ls:
            f_path = os.path.join(map_cache_path, i)
            # print("====")
            # print(f_path)
            # if f_path.find("png") != -1 and bool(re.search(r'\d', f_path)):
            if f_path.find("png") != -1:
                os.remove(f_path)
                # print("=========")
                # print(f_path)




#################################################栈###################################################

    
    
    # def GetMapFromCreate(self):
    #     '''
    #     等待最新map话题的地图数据
    #     '''
    #     try:
    #         start_time = time.time()
    #         msg_data = rospy.wait_for_message("/map", OccupancyGrid)
            
    #         map_size = msg_data.info.width * msg_data.info.height
    #         self.map_size = map_size
    #         self.map_originx = msg_data.info.origin.position.x
    #         self.map_originy = msg_data.info.origin.position.y
    #         self.map_width = msg_data.info.width
    #         self.map_height = msg_data.info.height

    #         # print(self.map_width,self.map_height)
    #         # map_np = np.zeros((msg_data.info.height,msg_data.info.width),dtype=np.uint8)
    #         # for y in range(msg_data.info.height):
    #         #     for x in range(msg_data.info.width):
    #         #         index = y * msg_data.info.width + x
    #         #         y_ = msg_data.info.height-y-1
    #         #         # y_ = y
    #         #         if msg_data.data[index] == -1: map_np[y_][x]=128
    #         #         elif msg_data.data[index] == 0: map_np[y_][x]=255
    #         #         else: map_np[y_][x]=0

    #         map_np = np.array(msg_data.data).reshape(msg_data.info.height,msg_data.info.width)
    #         map_np[map_np==-1]=128
    #         map_np[map_np==0]=255
    #         map_np[map_np==100]=0
    #         cv2.imwrite(self.__mapcache_path,map_np)
    #         map_np = cv2.flip(map_np,0)

    #         end_time = time.time()
    #         rospy.loginfo("[BulkdataParser]Total time of map generation<GetMapFromCreate>: {}s".format(end_time-start_time))
    #     except Exception as err:
    #         rospy.logerr("[BulkdataParser]Map generation error<GetMapFromCreate>: {}".format(str(err)))
    
    
    
'''
        
        # print("[BulkdataParser]map_database: {}".format(self.__map_database_dir))
        # print("[BulkdataParser]cache_database_path: {}".format(self.__mapcache_path))
        # rospy.Subscriber("/map",OccupancyGrid,self.Map_Callback,queue_size=1)
        ### 订阅机器人建图时map_cache_path
        # rospy.Subscriber("/map_cache_path",String,self.MapCachePath_Callback,queue_size=1)
        ###  订阅机器人建图时,地图长宽等信息
        # rospy.Subscriber("/MapMetaData_cache",MapMetaData,self.Map_Callback,queue_size=1)
'''

'''
    def TaskPathMap_Callback(self,msg_data):
        
        # 清扫任务路径（图）
        
        try:
            # 这个回调函数的话题一直在发布
            #if msg_data.info.width != 0:

            map_size = msg_data.info.width * msg_data.info.height
            if map_size == 0 or self.task.state == CleanTask.NEWBORN:
                if os.path.exists(self.__task_path_mapcache_path):
                    os.remove(self.__task_path_mapcache_path)  # 清空任务路径缓存
                # teach_mp = np.array([255 for i in range(100)]).reshape(10,10) # 地图背景数组
                # self.__task_path_mapcache_path = "/home/aoting/user_config/cache_database/task_teach_path_map_background.png"
                # cv2.imwrite(self.__task_path_mapcache_path,teach_mp)
                return
            # self.__task_path_mapcache_path = "/home/aoting/user_config/cache_database/task_path_map_cache.png"
            self.task_path_map_size = map_size
            self.task_path_map_originx = msg_data.info.origin.position.x
            self.task_path_map_originy = msg_data.info.origin.position.y
            self.task_path_map_width = msg_data.info.width
            self.task_path_map_height = msg_data.info.height
            
            #rospy.loginfo(self.task_path_map_width)

            map_np = np.array(msg_data.data).reshape(msg_data.info.height,msg_data.info.width)
            max_np = np.array([255 for i in range(map_size)]).reshape(msg_data.info.height,msg_data.info.width)
            # 二值图像：纯白255 纯黑0 
            # map_np[map_np==-1]=128
            # map_np[map_np==0]=255
            # map_np[map_np==100]=0
            # for x in np.nditer(map_np):
            #     print(x) 0 100

            map_np = max_np - map_np*2.55
            # for x in np.nditer(max_np):
            #     print(x) 
            map_np[map_np>255]=128
            #map_np=cv2.transpose(map_np)
            map_np = cv2.flip(map_np,0)
        
            # start_time = time.time()
            self.__clean_task_map_isupdate = True
            cv2.imwrite(self.__task_path_mapcache_path,map_np)  
            self.__clean_task_map_isupdate = False

            # end_time = time.time()
            # rospy.loginfo("[BulkdataParser][TaskPathMap_Callback]Total time of map generation: {}s".format(end_time-start_time))
        except Exception as err:
            rospy.logerr("[BulkdataParser][TaskPathMap_Callback]Map generation error: {}".format(str(err)))
'''

'''
    def Map_Callback(self,msg_data):
        
        # Map地图回调函数
        
        try:
            # 对应/map_index
            self.map_originx = msg_data.map_meta.origin.position.x
            self.map_originy = msg_data.map_meta.origin.position.y
            self.map_width = msg_data.map_meta.width
            self.map_height = msg_data.map_meta.height
            self.resolution = msg_data.map_meta.resolution

            self.__mapcache_path = msg_data.map_path

            # 除去当前上传的png图片，清空其他png图片
            # self.CleanMapCache2(self.__mapcache_path)
            # print("=======clean=======")


            
            # 对应map_cache_node 节点的话题/MapMetaData_cache
            # self.map_originx = msg_data.origin.position.x
            # self.map_originy = msg_data.origin.position.y
            # self.map_width = msg_data.width
            # self.map_height = msg_data.height

            # 对应/map话题
            # map_size = msg_data.info.width * msg_data.info.height
            # ##if self.map_size == map_size: return
            # self.map_size = map_size
            # self.map_originx = msg_data.info.origin.position.x
            # self.map_originy = msg_data.info.origin.position.y
            # self.map_width = msg_data.info.width
            # self.map_height = msg_data.info.height

            # map_np = np.array(msg_data.data).reshape(msg_data.info.height,msg_data.info.width)
            # max_np = np.array([255 for i in range(map_size)]).reshape(msg_data.info.height,msg_data.info.width)
            # # map_np[map_np==-1]=128
            # # map_np[map_np==0]=255
            # # map_np[map_np==100]=0
            # map_np = max_np - map_np*2.55
            # map_np[map_np>255]=128
            # map_np = cv2.flip(map_np,0)

            # # 如果正在建图，就写入缓存文件
            # print("==========Map_Callback==============")
            # print(self.__in_creating_map)
            # if self.__in_creating_map:
            #     # 对文件进行上锁解锁
            #     self.__map_isupdate = True
            #     cv2.imwrite(self.__mapcache_path,map_np)
            #     self.__map_isupdate = False
            
        except Exception as err:
            rospy.logerr("[BulkdataParser][Map_Callback]Map generation error: {}".format(str(err)))
'''